/**
 * @ Author: luoqi
 * @ Create Time: 2024-11-25 11:47
 * @ Modified by: luoqi
 * @ Modified time: 2024-11-25 18:10
 * @ Description:
 */
#include "qmath.h"

static const float _QFOC_FSIN_MAGIC = 0.017453292519943295769236907684886f;

static const float _sin_table[] = {
    0.0,                                    // sin(0)
    0.17364817766693034885171662676931 ,    // sin(10)
    0.34202014332566873304409961468226 ,    // sin(20)
    0.5 ,                                   // sin(30)
    0.64278760968653932632264340990726 ,    // sin(40)
    0.76604444311897803520239265055542 ,    // sin(50)
    0.86602540378443864676372317075294 ,    // sin(60)
    0.93969262078590838405410927732473 ,    // sin(70)
    0.98480775301220805936674302458952 ,    // sin(80)
    1.0                                     // sin(90)
};

static const float _cos_table[] = {
    1.0 ,                                   // cos(0)
    0.99984769515639123915701155881391 ,    // cos(1)
    0.99939082701909573000624344004393 ,    // cos(2)
    0.99862953475457387378449205843944 ,    // cos(3)
    0.99756405025982424761316268064426 ,    // cos(4)
    0.99619469809174553229501040247389 ,    // cos(5)
    0.99452189536827333692269194498057 ,    // cos(6)
    0.99254615164132203498006158933058 ,    // cos(7)
    0.99026806874157031508377486734485 ,    // cos(8)
    0.98768834059513772619004024769344      // cos(9)
};

static inline float _fmodf(float x, float y)
{
    return x - y * (int)(x / y);
}

static inline float _abs(float x)
{
    return x < 0.0f ? -x : x;
}

static inline float _max(float a, float b)
{
    return a > b ? a : b;
}

static inline float _min(float a, float b)
{
    return a < b ? a : b;
}

float fsind(float x)
{
    int s = 0;

    x = _fmodf(x, 360.0f);

    x = x < 0.0f ? x + 360.0f : x;

    if(x >= 180.0f) {
        s = -1;
        x = x - 180.0f;
    }

    x = (x > 90.0f) ? (180.0f - x) : x;

    int a = x * 0.1f;
    float b = x - 10 * a;

    float y = _sin_table[a] * _cos_table[(int)b] + b * _QFOC_FSIN_MAGIC * _sin_table[9 - a];

    y = (s < 0) ? -y : y;
    return y;
}

float fcosd(float x)
{
    return fsind(x + 90.0f);
}

float fsqrt(float x)
{
    union {
        float f;
        int i;
    } conv;
	float x2;
	x2 = x * 0.5f;
    conv.f = x;
    conv.i = 0x5f3504f3 - (conv.i >> 1);
    x = conv.f;
    x = x * (1.5f - (x2 * x * x)); // 1st iteration
    x = x * (1.5f - (x2 * x * x)); // 2nd iteration
    x = x * (1.5f - (x2 * x * x)); // 3nd iteration

    return 1.0f / x;
}

float fatan2(float y, float x)
{
    // a := min (|x|, |y|) / max (|x|, |y|)
    float abs_y = _abs(y);
    float abs_x = _abs(x);
    // inject FLT_MIN in denominator to avoid division by zero
    float a = _min(abs_x, abs_y) / (_max(abs_x, abs_y) + 1.17549e-38);
    // s := a * a
    float s = a * a;
    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
    float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
    // if |y| > |x| then r := 1.57079637 - r
    if(abs_y > abs_x) {
        r = 1.57079637f - r;
    }
    // if x < 0 then r := 3.14159274 - r
    if(x < 0.0f) {
        r = 3.14159274f - r;
    }
    // if y < 0 then r := -r
    if(y < 0.0f) {
        r = -r;
    }

    return r;
}

float rpm2deg(float rpm)
{
    return 6.0f * rpm;
}

float deg2rpm(float deg)
{
    return deg / 6.0f;
}
